/**
 ****************************************************************************************************
 * @file        foc_smo.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       滑模代码
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */

#include "foc_smo.h"
#include "foc_math.h"
#include "foc_filter.h"
#include <stdio.h>

#define SMO_Kslide	    1.868f				        //滑模增益K
#define SMO_Kslf		0.12566f			        //低通滤波系数

#define LPF_SMO_A		0.23905722361f				//低通滤波系数，截止频率1000Hz
#define LPF_SMO_B		0.76094277639f

//饱和函数
static float Sat(float s, float delta)
{
    if(s > delta)
        return 1;
    else if(s < -delta)
        return -1;
    else
        return s/delta;
}

void FOC_SMO_Init(FOC_SMO_t *foc_smo, float Rs, float Ls, float Ts, uint32_t pole_pairs, float max_speed)
{
    foc_smo->Rs = Rs;
    foc_smo->Ls = Ls;
    foc_smo->Rs_Ls = Rs/Ls;
    foc_smo->_1_Ls = 1.0f/Ls;
    foc_smo->Ts = Ts;

    foc_smo->speed.pole_pairs = pole_pairs;
    foc_smo->speed.Max_MecSpeed = max_speed;

    PLL_Init(&foc_smo->pll, Ts);
}


/* 滑模角度计算 */
void FOC_SMO_Angle_Calc(FOC_SMO_t *foc_smo, alphabeta_t Real_I, alphabeta_t Real_U)
{
    float Ts    = foc_smo->Ts;
    float Rs_Ls = foc_smo->Rs_Ls;
    float _1_Ls = foc_smo->_1_Ls;

    foc_smo->Real_U = Real_U;
    foc_smo->Real_I = Real_I;

    /* 积分计算观测电流 */
    foc_smo->Est_I.alpha += Ts*(-Rs_Ls * foc_smo->Est_I.alpha + _1_Ls*(foc_smo->Real_U.alpha - foc_smo->Est_V.alpha));
    foc_smo->Est_I.beta  += Ts*(-Rs_Ls * foc_smo->Est_I.beta  + _1_Ls*(foc_smo->Real_U.beta  - foc_smo->Est_V.beta));

    /* 计算误差 */
    foc_smo->Est_I_err.alpha = foc_smo->Est_I.alpha - foc_smo->Real_I.alpha;
    foc_smo->Est_I_err.beta  = foc_smo->Est_I.beta  - foc_smo->Real_I.beta;

    foc_smo->Est_V.alpha = foc_smo->h * Sat(foc_smo->Est_I_err.alpha, 0.5f);
    foc_smo->Est_V.beta  = foc_smo->h * Sat(foc_smo->Est_I_err.beta, 0.5f);

    //低通滤波
    foc_smo->Est_U.alpha = foc_smo->Est_U.alpha*0.1f + foc_smo->Est_V.alpha*0.9f;
    foc_smo->Est_U.beta  = foc_smo->Est_U.beta*0.1f  + foc_smo->Est_V.beta*0.9f;

    /* 反正切法求 */
    // foc_smo->atan_theta = Limit_Angle(atan2f(foc_smo->Est_U.alpha, foc_smo->Est_U.beta));
    // // foc_smo->atan_theta = Limit_Angle(arm_sin_f32(foc_smo->Est_U.beta, foc_smo->Est_U.alpha));
    // foc_smo->speed.ElAngle = foc_smo->atan_theta;
    
    /*锁相环法求*/
    PLL_Cale_SMO(&foc_smo->pll, foc_smo->speed.dir* foc_smo->Est_U.alpha, foc_smo->speed.dir*foc_smo->Est_U.beta);
//    foc_smo->speed.ElAngle = Limit_Angle(foc_smo->pll.Angle);        /*加上补偿后的角度*/
    foc_smo->speed.ElAngle = foc_smo->pll.Angle;
}

/* 滑模速度计算 */
void FOC_SMO_Speed_Calc(FOC_SMO_t *foc_smo)
{
    float tmp = Move_Filter_calculate(&foc_smo->pll.move_filter);
    tmp = tmp/foc_smo->speed.pole_pairs *60.0f/_2PI;
    foc_smo->speed.AvrMecSpeed = foc_smo->pll.move_filter.val /foc_smo->speed.pole_pairs *60.0f/_2PI;

    if(foc_smo->speed.AvrMecSpeed>foc_smo->speed.Max_MecSpeed){
        /*pll计算异常*/
        FOC_SMO_Clear(foc_smo);
        printf("滑模PLL计算异常\r\n");
    }
    
    // if(fabsf(foc_smo->speed.AvrMecSpeed)>1000){
    //     foc_smo->h = 0.1f;
    // }else{
    //     foc_smo->h = 0.1f;
    // }
    
}

/*滑模参数清零*/
void FOC_SMO_Clear(FOC_SMO_t *foc_smo)
{
    PLL_Clear(&foc_smo->pll);
}





